#include "includes.h"
#include "pocs.h"
#include "ScanData.h"

using namespace std;

void prepare(ScanData & s)
// function to be executed after the data has been entered in the DataScan object. After this function, the data is ready to start iterations
{
	int i,j,pa,pb;
	double ta,tb,y,G;
	bool b = false;
	// Set forward projection of unknown data (FBdata)
	
	for (j=0; j<s.Ny; j++){
		y = s.ymin + j*s.dy;
		ta = ShiftPosition( asin(y/s.R) );
		G = DetectorPosition(0, y, ta, s.R);
		tb = ShiftPosition(ta+2.*G+M_PI);
		pa = (int) PositionIndex(s ,ta,true);
		pb = (int) PositionIndex(s ,tb,false);

		for(i=0; i<s.Nx;i++){
			if(s.APmask[i][j]) {
				s.FPdata[j] = s.FPdata[j] + s.dx*s.APdata[i][j];
				b = true;
			}	
 		}
		//
		if(s.DBPmask[(int)s.Nx/2][j]){ 
			s.FPdata[j] = Fint(s, pa,G) - s.FPdata[j];
		}else{s.FPdata[j]=0;}
		b = false;
	}
	
	// Perform backprojection to set known Hilbert (DBPdata)
	backproj(s);
}

double DFint(const ScanData & s, const vector<vector<double> > & Ddata , int p, double G){
	double w1,w2, G1,G2;
	int m1,m2;
	
	m1 = (int) floor((G-s.gmin)/s.dg);
	m2 = (int) ceil((G-s.gmin)/s.dg);
	G1 = s.gmin + s.dg*m1;
	G2 = s.gmin + s.dg*m2;
	if(m1==m2){
		w1=0.5;
		w2=0.5;
	}else{
		w1 = (G - G2)/(G1-G2);
		w2 = (G - G1)/(G2-G1);
	}

	//cout<< m1 << " " << m2 << endl;	
	if (m2 < 0 || m2>=s.Ng){
		return (Ddata[p][m1]);
	}else if(m1 < 0 || m1>=s.Ng){
		return (Ddata[p][m2] );
	}else{
		return w1*(Ddata[p][m1]) +
			w2*(Ddata[p][m2] );
	}
}

double Fint(const ScanData & s, int p, double G){
	double w1,w2;
	int m1,m2;
	//M[0] = floor(U/du + (double)(Nu-1)/2.); // lower index
	m1 = (int) floor((G-s.gmin)/s.dg);
	//M[1] = ceil(U/du + (double)(Nu-1)/2.);
	m2 = (int) ceil((G-s.gmin)/s.dg);
	//cout<< M[0] << " " << M[1] << "\n";
	//double U0 = du*((double) M[0] - (Nu-1)/2.);
	//double U1 = du*((double) M[1] - (Nu-1)/2.);
	double G0 = s.gmin + s.dg*((double)m1);
	double G1 = s.gmin + s.dg*((double)m2);
	
	if(m1==m2){
		w1=0.5;
		w2=0.5;
	}else{
		w1 = (G - G1)/(G0-G1);
		w2 = (G - G0)/(G1-G0);
	}

	if (m2 < 0 || m2>=s.Ng){
		return (s.data[p][m1]);
	}else if(m1 < 0 || m1>=s.Ng){
		return (s.data[p][m2] );
	}else{
		return w1*(s.data[p][m1]) +
			w2*(s.data[p][m2] );
	}	
}

void backproj(ScanData & s)
// sets the backprojected data.
{
	int p=0; // t index
	int n=0; // u index
	int i=0; // x index
	int j=0; // y index
	int pi,pf,p1;
	double x=0,y=0,t=0,L=0,G=0,ta=0,tb=0;

	vector<vector<double> >  Ddata (s.Nt, vector<double> (s.Ng,0));

	// Differentiate
	//Gamma derivative
	for(p=0; p<s.Nt;++p){ // t loop
		Ddata[p][0] = (-3./2.*s.data[p][0] + 2.*s.data[p][1] - 1./2.*s.data[p][2])/s.dg;
		for(n=1; n<s.Ng-1;++n){ // g loop
			Ddata[p][n] = (-1./2.*s.data[p][n-1] + 1./2.*s.data[p][n+1])/s.dg;
		}
		Ddata[p][s.Ng-1] = (3./2.*s.data[p][s.Ng-1] - 2.*s.data[p][s.Ng-2] + 1./2.*s.data[p][s.Ng-3])/s.dg;
	}
	
	//t derivative
	
	for(n=0; n<s.Ng;++n){ // g loop
		Ddata[0][n] = (-3./2.*s.data[0][n] + 2.*s.data[1][n] - 1./2.*s.data[2][n])/s.dt - Ddata[0][n];
		for(p=1; p<s.Nt-1;++p){ // t loop
			Ddata[p][n] = (-1./2.*s.data[p-1][n] + 1./2.*s.data[p+1][n])/s.dt - Ddata[p][n];
		}
		Ddata[s.Nt-1][n] = (3./2.*s.data[s.Nt-1][n] - 2.*s.data[s.Nt-2][n] + 1./2.*s.data[s.Nt-3][n])/s.dt - Ddata[s.Nt-1][n];
	}
	
	// Perform backprojection
	//for(j=0; j<s.Ny;++j){ // y loop
	//	y = s.ymin + s.dy * j;
	//	ta = asin(y/s.R);
	//	pa = (int) ceil((ta - s.tmin)/s.dt);
	//	tb = M_PI - ta;
	//	pb = (int) floor((tb - s.tmin)/s.dt);
	//	//cout<<y << " " << tb<< " " << pa << " " << pb <<endl;
	//	for(i=0; i<s.Nx;++i){ // x loop
	//		x = s.xmin + s.dx * (i);

	//		for(p=pa; p<=pb;++p){
	//			t = s.tmin + s.dt * p;
	//			L = sqrt( pow(x - s.R*cos(t),2) + pow(y - s.R*sin(t),2) );
				//G = atan3(y - s.R*sin(t) , x - s.R*cos(t) ) - t - M_PI;
				//cout <<t<< endl;
				//cout<<G<<endl;
				//if ((G<s.gmax) && (G>s.gmin) && t<s.tmax && t>s.tmin){ 
				//	s.DBPdata[i][j] = s.DBPdata[i][j] + s.dt/L * DFint(s, Ddata, p,G);
				//	
				//}else{
				//	s.DBPdata[i][j] = 0;
				//	break;
				//}
				
			//}

		//}
	//}-
	// Perform backprojection
	for(j=0; j<s.Ny;++j){ // y loop
		y = s.ymin + s.dy * j;
		
		ta = ShiftPosition( asin(y/s.R) );
		G = DetectorPosition(0, y, ta, s.R);
		tb = ShiftPosition(ta+2.*G+M_PI);
		pi = (int) PositionIndex(s ,ta,true);
		pf = (int) PositionIndex(s ,tb,false);
		//cout <<j <<", "<< ta <<", "<< IndexPosition(*this,pi) <<", "<<  tb << ", " << IndexPosition(*this,pf) << ", "<< pi << ", " <<pf << endl;
		for(i=0; i<s.Nx;++i){ // x loop
			x = s.xmin + s.dx * i;
			//if (isHilbertKnown(*this, x, y)){
			if(s.DBPmask[i][j])
				if(isMissing(s,ta)){
					project(s, Ddata, s.DBPdata[i][j], 0,  pf,  x,  y, false);
					pi = (int) ceil((ta-s.tmin)/s.dt);
					project(s, Ddata, s.DBPdata[i][j], pi,  -1,  x,  y, true);
				}else if(isMissing(s,tb)){
					project(s, Ddata, s.DBPdata[i][j], pi,  s.Nt-1,  x,  y, false);
					if(tb<0) tb = tb + 2.* M_PI;
					pf = (int) ceil((tb-s.tmax)/s.dt);
					project(s, Ddata,s.DBPdata[i][j], s.Nt,  pf+s.Nt-1,  x,  y, true);
				}else{
					if(pi>pf){
						project(s, Ddata, s.DBPdata[i][j], pi,  s.Nt-1,  x,  y, false);
						p1 = (int) floor((s.tmin-s.tmax)/s.dt); //might need to be improved
						project(s, Ddata, s.DBPdata[i][j], s.Nt,  s.Nt+p1-1,  x,  y, true);
						project(s, Ddata, s.DBPdata[i][j], 1,  pf,  x,  y, false);
						
					}else{
						project(s, Ddata, s.DBPdata[i][j], pi,  pf,  x,  y, false);
					}
				}
			
		}
	}

}

double atan3(double y,double x)
{
	double a = atan2(y,x);
	if (a>=0){ return a;}
	else{ return 2.*M_PI+a;}
}

double ShiftPosition(const double t){
	double T = fmod((double) t + 9.*M_PI,2.*M_PI);
	return T - M_PI;
}

double DetectorPosition(double x, double y, double t, double R){
	
	double phi = atan3(y-R*sin(t) , x-R*cos(t) );
	if( t<=M_PI && t>=M_PI_2 && phi < M_PI ){
		return phi - t + M_PI;
	}else if(t>-M_PI && t<=-M_PI_2 && phi>=M_PI ){
		return phi - t - 3.* M_PI;
	}else{
		return phi - t - M_PI;
	}
}

double IndexPosition(const ScanData &s, const int p){
	return ShiftPosition(s.tmin + s.dt * p);
}

int PositionIndex(const ScanData &s, const double t, bool isCeil){
	double T =(t-s.tmin);
	if ((ShiftPosition(t)<s.tmin||ShiftPosition(t)>s.tmin+M_PI)&&!(s.tmin<0 && t>ShiftPosition(M_PI+s.tmin))) T= 2.*M_PI+ T;
	
	int p;
	if(isCeil){p = (int) ceil(fmod((double)(T)/s.dt + 8.*(s.Nt),s.Nt));}
	else{p = (int) floor(fmod((double)(T)/s.dt + 8.*(s.Nt),s.Nt));}
	return p;
}

bool isMissing(const ScanData & s, const double t)
{
	if(s.tmin < 0 && s.tmax < 0){
		if(s.tmin > s.tmax && t>s.tmax && t<s.tmin){ return true;}
		else if(s.tmin < s.tmax && (t>s.tmax || t<s.tmin)){ return true;}
		else {return false;}
	}else if(s.tmin >= 0 && s.tmax >= 0){
		if(s.tmin > s.tmax && t>s.tmax && t<s.tmin){ return true;}
		else if(s.tmin < s.tmax && (t>s.tmax || t<s.tmin)){ return true;}
		else {return false;}
	}else if(s.tmin>=0 && s.tmax<0){
		if(t>s.tmax && t<s.tmin){return true;}
		else{return false;}
	}else if(s.tmin< 0 && s.tmax>=0){
		if(t>s.tmax || t<s.tmin){return true;}
		else{return false;}
	}else{
		return false;
	}
}

bool isHilbertKnown(const ScanData & s, double x, double y)
{
	if (x*x + y*y < s.rov*s.rov){ //return true;
		double medang = (s.tmin + s.tmax)/2.;
		if(s.tmax>s.tmin) medang = medang + M_PI;

		double k[2] = {cos(medang), sin(medang)};
		double C = k[0]*s.R*cos(s.tmax) + k[1]*s.R*sin(s.tmax);

		if(k[0]*x + k[1]*y<C){return true;}
		else{return false;}
	}else{
		return false;
	}
}

void project(ScanData & s, const vector<vector<double> > & Ddata, double & image, int pi, int pf, double x, double y, bool isConj){
	int p=0,p1=0;
	double t, L, G;
	if(isConj){
		for(p=pi;p<=pf;p++){
			t = ShiftPosition(s.tmin + p*s.dt);
			L = sqrt( pow(x - s.R*cos(t),2) + pow(y - s.R*sin(t),2) );
			G = DetectorPosition(x, y, t, s.R);			
			t = ShiftPosition(t + 2.*G + M_PI);
			p1 = PositionIndex(s ,t,true);
			image = image - s.dt/L * DFint(s, Ddata, p1,-G);
		}
	}else{
		for(p=pi; p<=pf;++p){
			t = IndexPosition(s,p); 
			L = sqrt( pow(x - s.R*cos(t),2) + pow(y - s.R*sin(t),2) );
			G = DetectorPosition(x, y, t, s.R);
			image = image + s.dt/L * DFint(s, Ddata,p, G);
		}
	}
}
